import numpy as np
import matplotlib.pyplot as plt
# 输入的路径
path = [(4.612602, -0.085868), (8.077701, -0.07499), (9.846517, 1.95458), (9.873239, 13.887309),
        (11.929904, 16.029049), (12.654362, 16.029049), (13.724722, 16.029049), (14.812855, 16.029049),
        (15.729413, 16.029049), (16.983881, 16.032003), (18.217407, 18.472281), (18.169516, 28.700434)]

# 计算两点之间的欧氏距离
def euclidean_distance(p1, p2):
    return np.linalg.norm(np.array(p2) - np.array(p1))
# 将路径拆分为等距的片段

path = [(0, 0), (3, 4), (7, 1), (10, 5)]

def split_path_equally(path, step_size):
    new_path = [path[0]]  # 添加初始点
    for i in range(len(path)-1):
        p1 = np.array(path[i])
        p2 = np.array(path[i+1])
        segment_dist = euclidean_distance(p1, p2)
        segment_vector = (p2 - p1) / segment_dist
        num_steps = int(segment_dist / step_size)  # 根据步长计算需要分割的步数
        for j in range(1, num_steps + 1):
            new_point = p1 + j * (segment_dist / num_steps) * segment_vector  # 计算新的点坐标
            new_path.append(tuple(new_point))

    if tuple(path[-1]) not in new_path:
        new_path.append(tuple(path[-1]))
    return new_path

if __name__ == '__main__':
    # 用例：定义要模拟的路径片段数量
    step_size = 1

    # 拆分路径
    segmented_path = split_path_equally(path, step_size)

    # 输出结果
    print(segmented_path)
    plt.ion()  # 启用交互模式
    fig = plt.figure()
    a = []
    b = []
    for p in segmented_path:
        a.append(p[0])
        b.append(p[1])

        plt.scatter(a, b, s=10, color='green')
        plt.draw()
        plt.pause(0.1)

    plt.ioff()  # 退出交互模式
    plt.show()